#!/usr/bin/env python3
# robots/robot_controller.py
# 单机器人控制器：通信、位置管理
import socket
import threading
import time
import struct
import zlib
import binascii
from math import dist
import logging

from config import (
    FRAME_SIZE,
    VERSION,
    ROBOT_TIMEOUT,
    COMMAND_TYPES,
    ROLES,
    STATUS_CODES,
)
from utils.frame_builder import build_frame, parse_frame, recv_exact  # 工具导入

logger = logging.getLogger(__name__)


class RobotController:
    def __init__(self, robot_id, robot_name, role, client_socket, addr, server):
        self.robot_id = robot_id
        self.robot_name = robot_name
        self.role = role
        self.client_socket = client_socket
        self.addr = addr
        self.current_position = None
        self.initial_position = None
        self.current_node = None
        self.target_position = None
        self.target_node = None
        self.last_update_time = time.time()
        self.last_heartbeat_time = time.time()
        self.status = "connecting"
        self.active = False
        self.debug_mode = False
        self.server = server
        self.last_target_send_time = time.time()
        self.last_position = None
        # 启动接收线程
        self.receive_thread = threading.Thread(
            target=self.receive_messages, daemon=True
        )
        self.receive_thread.start()
        # 发送角色分配
        self.send_role_assignment()

    def send_role_assignment(self):
        """发送角色分配"""
        role_code = ROLES[self.role]
        payload = struct.pack(">B", role_code) + bytes(13)
        frame = build_frame(COMMAND_TYPES["RoleAssignment"], payload, self.robot_id)
        try:
            self.client_socket.sendall(frame)
            logger.info(f"[Robot-{self.robot_id}] 已分配角色: {self.role}")
            return True
        except Exception as e:
            logger.error(f"[Robot-{self.robot_id}] 发送角色失败: {e}")
            return False

    def update_position(self, x, y):
        """更新位置并检测变化"""
        old_position = self.current_position
        self.current_position = (x, y)
        if self.initial_position is None:
            self.initial_position = (x, y)
            logger.info(f"[Robot-{self.robot_id}] 记录初始位置: ({x:.2f}, {y:.2f})")
        old_node = self.current_node
        self.current_node = self.server.map_manager.position_to_node(x, y)
        self.last_update_time = time.time()
        self.status = "active"
        self.last_heartbeat_time = time.time()

        if (
            old_node != self.current_node
            and self.is_active()
            and self.server.game_active
        ):
            self.server.on_robot_node_changed()
            logger.debug(
                f"[Robot-{self.robot_id}] 节点变化: {old_node} -> {self.current_node}"
            )

        if old_position is not None and self.server.game_active:
            old_x, old_y = old_position
            pos_dist = dist((old_x, old_y), (x, y))
            if pos_dist > 0.05 and self.is_active():
                self.server.on_robot_node_changed()
                logger.debug(f"[Robot-{self.robot_id}] 位置变化: {pos_dist:.3f}m")

    def update_target(self, target_node):
        """更新目标"""
        self.target_node = target_node
        self.target_position = self.server.map_manager.node_to_position(target_node)
        self.last_update_time = time.time()

    def send_target(self):
        """发送目标位置"""
        if self.target_position is None or self.client_socket is None:
            return False
        try:
            x, y = self.target_position
            payload = struct.pack(">fff", x, y, 30) + bytes(2)
            frame = build_frame(COMMAND_TYPES["NavigationGoal"], payload, self.robot_id)
            self.client_socket.sendall(frame)
            self.last_target_send_time = time.time()
            return True
        except Exception as e:
            logger.error(f"[Robot-{self.robot_id}] 发送目标失败: {e}")
            self.status = "disconnected"
            return False

    def send_heartbeat(self):
        """发送心跳"""
        if self.client_socket is None:
            return False
        try:
            seq = int(time.time() * 1000) % 1000000
            payload = struct.pack(">i", seq) + bytes(10)
            frame = build_frame(COMMAND_TYPES["Heartbeat"], payload, self.robot_id)
            self.client_socket.sendall(frame)
            return True
        except Exception as e:
            logger.error(f"[Robot-{self.robot_id}] 发送心跳失败: {e}")
            self.status = "disconnected"
            return False

    def receive_messages(self):
        """接收消息"""
        try:
            while self.status != "disconnected":
                data = recv_exact(self.client_socket, FRAME_SIZE)
                if not data:
                    logger.info(f"[Robot-{self.robot_id}] 连接断开")
                    self.status = "disconnected"
                    break
                if data == b"":
                    if time.time() - self.last_heartbeat_time > ROBOT_TIMEOUT:
                        logger.warning(f"[Robot-{self.robot_id}] 心跳超时")
                        self.status = "disconnected"
                        break
                    continue
                frame = parse_frame(data)
                if not frame:
                    continue
                if frame["robot_id"] != self.robot_id:
                    logger.warning(
                        f"[Robot-{self.robot_id}] 错误ID: {frame['robot_id']}"
                    )
                    continue
                if frame["command"] == COMMAND_TYPES["Heartbeat"]:
                    self.last_heartbeat_time = time.time()
                    self.send_heartbeat()
                    continue
                if frame["command"] == COMMAND_TYPES["PositionReport"]:
                    try:
                        x, y, status = struct.unpack(">fff", frame["payload"][:12])
                        self.update_position(x, y)
                        logger.debug(
                            f"[Robot-{self.robot_id}] 位置更新: ({x:.2f},{y:.2f})"
                        )
                    except struct.error as e:
                        logger.error(f"[Robot-{self.robot_id}] 位置解析错误: {e}")
                        continue
                elif frame["command"] == COMMAND_TYPES["CommandAck"]:
                    try:
                        acked_command, status_code = struct.unpack(
                            ">BB", frame["payload"][:2]
                        )
                        status_msg = next(
                            (k for k, v in STATUS_CODES.items() if v == status_code),
                            "Unknown",
                        )
                        logger.debug(f"[Robot-{self.robot_id}] 确认: {status_msg}")
                    except struct.error:
                        pass
                else:
                    logger.warning(
                        f"[Robot-{self.robot_id}] 未知指令: {frame['command']}"
                    )
        except Exception as e:
            logger.warning(f"[Robot-{self.robot_id}] 连接异常: {e}")
            self.status = "disconnected"
        finally:
            if self.client_socket:
                self.client_socket.close()
                self.client_socket = None

    def get_info(self):
        """获取信息"""
        pos_str = (
            f"({self.current_position[0]:.1f}, {self.current_position[1]:.1f})"
            if self.current_position
            else "未知"
        )
        target_str = (
            f"({self.target_position[0]:.1f}, {self.target_position[1]:.1f})"
            if self.target_position
            else "未设置"
        )
        node_str = str(self.current_node) if self.current_node is not None else "未映射"
        target_node_str = (
            str(self.target_node) if self.target_node is not None else "未设置"
        )
        initial_str = (
            f"({self.initial_position[0]:.1f}, {self.initial_position[1]:.1f})"
            if self.initial_position
            else "未记录"
        )
        return {
            "id": self.robot_id,
            "name": self.robot_name,
            "role": self.role,
            "position": pos_str,
            "initial_position": initial_str,
            "target": target_str,
            "node": node_str,
            "target_node": target_node_str,
            "status": self.status,
            "last_update": time.time() - self.last_update_time,
            "last_heartbeat": time.time() - self.last_heartbeat_time,
        }

    def is_active(self):
        """检查活跃"""
        if self.status == "disconnected":
            return False
        if time.time() - self.last_heartbeat_time > ROBOT_TIMEOUT * 2:
            self.status = "disconnected"
            return False
        return (
            self.status == "active"
            and (time.time() - self.last_update_time) < ROBOT_TIMEOUT
        )

    def disconnect(self):
        """断开"""
        self.status = "disconnected"
        if self.client_socket:
            try:
                self.client_socket.shutdown(socket.SHUT_RDWR)
            except:
                pass
            self.client_socket.close()
            self.client_socket = None
